#include "final.h"
#include "route.h"
#include "adc.h"
#include "route.h"
#include "motor.h"
#include "servo.h"
#include "digital.h"
#include "delay.h"
#include "compass.h"
#include "voice.h"
#include "lcdtest.h"
/**
 * @brief door1 2 3 4 用于存储四个门颜色, 默认返回错误0
 */
uint8_t door1 = 0, door2 = 0, door3 = 0, door4 = 0;
/**
 * @brief 只有绿门才走 红黄都不走
 */
void Complete_Route1(void)
{
    // 第一趟 判门
    start();
    p1_p2(1);
    p2_p4();
    p4_p3();
    p3_door2();
    stop();
    delay_ms(1000);
    // 2门是绿
    if (Color_Value() == 1) {
        door2 = 1;
        door2_p5();
        p5_jd5_p7();
        p7_p8();
        p8_jd3();
        jd3_jd4();
        jd4_door2_home();
        stop();
    } else {
        // 2门是红门或者黄门 去1门
        door2_door1();
        stop();
        delay_ms(1000);
        // 1门是绿
        if (Color_Value() == 1) {
            door1 = 1;
            door1_jd4_p5();
            p5_jd5_p7();
            p7_p8();
            p8_jd3();
            jd3_door1_home();
            stop();
        } else {
            // 1门是红门或者黄门 去4门
            door1_door4();
            stop();
            delay_ms(800);
            // 4门是绿
            if (Color_Value() == 1) {
                door4 = 1;
                door4_p5();
                p5_jd5_p7();
                p7_p8();
                p8_jd3();
                jd3_jd4();
                jd4_door4_home();
                stop();
            } else {
                // 4门不是绿色, 3门是绿色
                // 3门是绿
                door3 = 1;
                door4_door3_p5();
                p5_jd5_p7();
                p7_p8();
                p8_jd3();
                jd3_jd4();
                jd4_door3_home();
                stop();
            }
        }
    }

    // 第二趟 不判门
    start();
    p1_p2(2);
    p2_p4();
    p4_p3();
    if (door1 == 1) {
        p3_door1();
        door1_jd4_p5();
        p5_jd5_p7();
        p7_p8();
        p8_jd3();
        jd3_door1_home();
        stop();
        Showsensors1();
    } else if (door2 == 1) {
        p3_door2_p5();
        p5_jd5_p7();
        p7_p8();
        p8_jd3();
        jd3_jd4();
        jd4_door2_home();
        stop();
        Showsensors1();
    } else if (door3 == 1) {
        p3_door3_p5();
        p5_jd5_p7();
        p7_p8();
        p8_jd3();
        jd3_jd4();
        jd4_door3_home();
        stop();
        Showsensors1();
    } else if (door4 == 1) {
        p3_door4();
        door4_p5();
        p5_jd5_p7();
        p7_p8();
        p8_jd3();
        jd3_jd4();
        jd4_door4_home();
        stop();
        Showsensors1();
    }
}